﻿// <copyright file="Matrix3D.cs" company="Axelerate Solutions S.A.">
// Copyright (c) Axelerate Solutions S.A.  All rights reserved.
// </copyright>

namespace Axelerate.Silverlight3D.Media.Media3D
{
    using System;
    using System.Net;
    using System.Windows;
    using System.Windows.Controls;
    using System.Windows.Documents;
    using System.Windows.Ink;
    using System.Windows.Input;
    using System.Windows.Media;
    using System.Windows.Media.Animation;
    using System.Windows.Shapes;
    using System.ComponentModel;
using System.Collections.Generic;

    /// <summary>
    /// File name: Matrix3D.cs
    /// Company: Axelerate Solutions S.A.
    /// Created By: Daniel Rojas
    /// Created Date: 01/30/2008 16:12:00 h.
    /// Description: Represents a 4 x 4 matrix used for transformations in 3-D space.
    ///   <Modifications>
    ///   </Modifications>
    /// </summary>
    [TypeConverterAttribute(typeof(Matrix3DConverter))]
    public struct Matrix3D : IFormattable
    {
        #region Fields
        /// <summary>
        /// Gets or sets the value of the first row and first column of this Matrix3D structure.
        /// </summary>
        private double mM11;

        /// <summary>
        /// Gets or sets the value of the first row and second column of this Matrix3D structure.
        /// </summary>
        private double mM12;

        /// <summary>
        /// Gets or sets the value of the first row and third column of this Matrix3D structure.
        /// </summary>
        private double mM13;

        /// <summary>
        /// Gets or sets the value of the first row and fourth column of this Matrix3D structure.
        /// </summary>
        private double mM14;

        /// <summary>
        /// Gets or sets the value of the second row and first column of this Matrix3D structure.
        /// </summary>
        private double mM21;

        /// <summary>
        /// Gets or sets the value of the second row and second column of this Matrix3D structure.
        /// </summary>
        private double mM22;

        /// <summary>
        /// Gets or sets the value of the second row and third column of this Matrix3D structure.
        /// </summary>
        private double mM23;

        /// <summary>
        /// Gets or sets the value of the second row and fourth column of this Matrix3D structure.
        /// </summary>
        private double mM24;

        /// <summary>
        /// Gets or sets the value of the third row and first column of this Matrix3D structure.
        /// </summary>
        private double mM31;

        /// <summary>
        /// Gets or sets the value of the third row and second column of this Matrix3D structure.
        /// </summary>
        private double mM32;

        /// <summary>
        /// Gets or sets the value of the third row and third column of this Matrix3D structure.
        /// </summary>
        private double mM33;

        /// <summary>
        /// Gets or sets the value of the third row and fourth column of this Matrix3D structure.
        /// </summary>
        private double mM34;

        /// <summary>
        /// Gets or sets the value of the fourth row and first column of this Matrix3D structure
        /// </summary>
        private double mOffsetX;

        /// <summary>
        /// Gets or sets the value of the fourth row and second column of this Matrix3D structure
        /// </summary>
        private double mOffsetY;

        /// <summary>
        /// Gets or sets the value of the fourth row and third column of this Matrix3D structure.
        /// </summary>
        private double mOffsetZ;

        /// <summary>
        /// Gets or sets the value of the fourth row and fourth column of this Matrix3D structure.
        /// </summary>
        private double mM44;

        #endregion

        #region Properties

        private bool _IsDefaultMatrix;

        /// <summary>
        /// Gets or sets the value of the first row and first column of this Matrix3D structure.
        /// </summary>
        public double M11
        {
            get
            {
                return this.mM11;
            }
            set
            {
                this.mM11 = value;
                this._IsDefaultMatrix = false;
            }
        }
        
        /// <summary>
        /// Gets or sets the value of the first row and second column of this Matrix3D structure.
        /// </summary>
        public double M12
        {
            get
            {
                return this.mM12;
            }
            set
            {
                this.mM12 = value;
                this._IsDefaultMatrix = false;
            }
        }

        /// <summary>
        /// Gets or sets the value of the first row and third column of this Matrix3D structure.
        /// </summary>
        public double M13
        {
            get
            {
                return this.mM13;
            }
            set
            {
                this.mM13 = value;
                this._IsDefaultMatrix = false;
            }
        }

        /// <summary>
        /// Gets or sets the value of the first row and fourth column of this Matrix3D structure.
        /// </summary>
        public double M14
        {
            get
            {
                return this.mM14;
            }
            set
            {
                this.mM14 = value;
                this._IsDefaultMatrix = false;
            }
        }

        /// <summary>
        /// Gets or sets the value of the second row and first column of this Matrix3D structure.
        /// </summary>
        public double M21
        {
            get
            {
                return this.mM21;
            }
            set
            {
                this.mM21 = value;
                this._IsDefaultMatrix = false;
            }
        }

        /// <summary>
        /// Gets or sets the value of the second row and second column of this Matrix3D structure.
        /// </summary>
        public double M22
        {
            get
            {
                return this.mM22;
            }
            set
            {
                this.mM22 = value;
                this._IsDefaultMatrix = false;
            }
        }

        /// <summary>
        /// Gets or sets the value of the second row and third column of this Matrix3D structure.
        /// </summary>
        public double M23
        {
            get
            {
                return this.mM23;
            }
            set
            {
                this.mM23 = value;
                this._IsDefaultMatrix = false;
            }
        }

        /// <summary>
        /// Gets or sets the value of the second row and fourth column of this Matrix3D structure.
        /// </summary>
        public double M24
        {
            get
            {
                return this.mM24;
            }
            set
            {
                this.mM24 = value;
                this._IsDefaultMatrix = false;
            }
        }

        /// <summary>
        /// Gets or sets the value of the third row and first column of this Matrix3D structure.
        /// </summary>
        public double M31
        {
            get
            {
                return this.mM31;
            }
            set
            {
                this.mM31 = value;
                this._IsDefaultMatrix = false;
            }
        }

        /// <summary>
        /// Gets or sets the value of the third row and second column of this Matrix3D structure.
        /// </summary>
        public double M32
        {
            get
            {
                return this.mM32;
            }
            set
            {
                this.mM32 = value;
                this._IsDefaultMatrix = false;
            }
        }

        /// <summary>
        /// Gets or sets the value of the third row and third column of this Matrix3D structure.
        /// </summary>
        public double M33
        {
            get
            {
                return this.mM33;
            }
            set
            {
                this.mM33 = value;
                this._IsDefaultMatrix = false;
            }
        }

        /// <summary>
        /// Gets or sets the value of the third row and fourth column of this Matrix3D structure.
        /// </summary>
        public double M34
        {
            get
            {
                return this.mM34;
            }
            set
            {
                this.mM34 = value;
                this._IsDefaultMatrix = false;
            }
        }

        /// <summary>
        /// Gets or sets the value of the fourth row and first column of this Matrix3D structure
        /// </summary>
        public double OffsetX
        {
            get
            {
                return this.mOffsetX;
            }
            set
            {
                this.mOffsetX = value;
                this._IsDefaultMatrix = false;
            }
        }

        /// <summary>
        /// Gets or sets the value of the fourth row and second column of this Matrix3D structure.
        /// </summary>
        public double OffsetY
        {
            get
            {
                return this.mOffsetY;
            }
            set
            {
                this.mOffsetY = value;
                this._IsDefaultMatrix = false;
            }
        }

        /// <summary>
        /// Gets or sets the value of the fourth row and third column of this Matrix3D structure.
        /// </summary>
        public double OffsetZ
        {
            get
            {
                return this.mOffsetZ;
            }
            set
            {
                this.mOffsetZ = value;
                this._IsDefaultMatrix = false;
            }
        }
        
        /// <summary>
        /// Gets or sets the value of the fourth row and fourth column of this Matrix3D structure.
        /// </summary>
        public double M44
        {
            get
            {
                return this.mM44;
            }
            set
            {
                this.mM44 = value;
                this._IsDefaultMatrix = false;
            }
        }

        /// <summary>
        /// Gets a value that indicates whether this Matrix3D is invertible.
        /// </summary>
        /// <value>true if the Matrix3D structure has an inverse; otherwise, false. The default value is true.</value>
        public bool HasInverse
        {
            get
            {
                double determinant = this.Determinant;
                if (System.Math.Round(determinant, 4) == 0)
                {
                    return false;
                }
                else
                {
                    return true;
                }
            }
        }

        /// <summary>
        /// Retrieves the determinant of this Matrix3D structure.
        /// </summary>
        /// <value>The determinant of this Matrix3D structure.</value>
        public double Determinant
        {
            get
            {
                return  mM14 * mM23 * mM32 * mOffsetX - mM13 * mM24 * mM32 * mOffsetX -
                        mM14 * mM22 * mM33 * mOffsetX + mM12 * mM24 * mM33 * mOffsetX +
                        mM13 * mM22 * mM34 * mOffsetX - mM12 * mM23 * mM34 * mOffsetX -
                        mM14 * mM23 * mM31 * mOffsetY + mM13 * mM24 * mM31 * mOffsetY +
                        mM14 * mM21 * mM33 * mOffsetY - mM11 * mM24 * mM33 * mOffsetY -
                        mM13 * mM21 * mM34 * mOffsetY + mM11 * mM23 * mM34 * mOffsetY +
                        mM14 * mM22 * mM31 * mOffsetZ - mM12 * mM24 * mM31 * mOffsetZ -
                        mM14 * mM21 * mM32 * mOffsetZ + mM11 * mM24 * mM32 * mOffsetZ +
                        mM12 * mM21 * mM34 * mOffsetZ - mM11 * mM22 * mM34 * mOffsetZ -
                        mM13 * mM22 * mM31 * mM44 + mM12 * mM23 * mM31 * mM44 +
                        mM13 * mM21 * mM32 * mM44 - mM11 * mM23 * mM32 * mM44 -
                        mM12 * mM21 * mM33 * mM44 + mM11 * mM22 * mM33 * mM44;
            }
        }

        /// <summary>
        /// Changes a Matrix3D structure into an identity Matrix3D.
        /// </summary>
        /// <value>
        /// The identity Matrix3D.
        /// </value>
        /// <remarks>
        /// In an identity matrix, all coefficients are 0 except the [1,1][2,2][3,3][4,4] coefficients, which are set to 1. The identity matrix is special in that when it is applied to vertices, they are unchanged. The identity matrix is used as the starting point for matrices that modify vertex values to create rotations, translations, and any other transformations that can be represented by a 4x4 matrix.
        /// </remarks>
        public static Matrix3D Identity;
        

        /// <summary>
        /// Determines whether this Matrix3D structure is an identity Matrix3D.
        /// </summary>
        /// <value>
        /// true if the Matrix3D structure is an identity Matrix3D; otherwise, false. The default value is true.
        /// </value>
        /// <remarks>
        /// In an identity matrix, all coefficients are 0 except the [1,1][2,2][3,3][4,4] coefficients, which are set to 1. The identity matrix is special in that when it is applied to vertices, they are unchanged. The identity matrix is used as the starting point for matrices that modify vertex values to create rotations, translations, and any other transformations that can be represented by a 4x4 matrix.
        /// </remarks>
        public bool IsIdentity
        {
            get
            {
                if (this._IsDefaultMatrix)
                {
                    return true;
                }
                else
                {
                    return this.Equals(Matrix3D.Identity);
                }
            }
        }

        /// <summary>
        /// Gets a value that indicates whether this Matrix3D structure is affine.
        /// </summary>
        /// <value>true if the Matrix3D structure is affine; otherwise, false.</value>
        public bool IsAffine
        {
            get
            {
                if (this.IsIdentity)
                {
                    return true;
                }
                else
                {
                    return this.mM14 == 0 &&
                           this.mM24 == 0 &&
                           this.mM34 == 0 &&
                           this.mM44 == 1;
                }
            }
        }

        #endregion 

        #region Constructor

        static Matrix3D()
        {
            Identity = new Matrix3D(1, 0, 0, 0,
                                    0, 1, 0, 0,
                                    0, 0, 1, 0,
                                    0, 0, 0, 1);
            Identity._IsDefaultMatrix = true;
        }

        /// <summary>
        /// Constructor that sets matrix's initial values.
        /// </summary>
        /// <param name="m11">Value of the (1,1) field of the new matrix.</param>
        /// <param name="m12">Value of the (1,2) field of the new matrix.</param>
        /// <param name="m13">Value of the (1,3) field of the new matrix.</param>
        /// <param name="m14">Value of the (1,4) field of the new matrix.</param>
        /// <param name="m21">Value of the (2,1) field of the new matrix.</param>
        /// <param name="m22">Value of the (2,2) field of the new matrix.</param>
        /// <param name="m23">Value of the (2,3) field of the new matrix.</param>
        /// <param name="m24">Value of the (2,4) field of the new matrix.</param>
        /// <param name="m31">Value of the (3,1) field of the new matrix.</param>
        /// <param name="m32">Value of the (3,2) field of the new matrix.</param>
        /// <param name="m33">Value of the (3,3) field of the new matrix.</param>
        /// <param name="m34">Value of the (3,4) field of the new matrix.</param>
        /// <param name="offsetX">Value of the X offset field of the new matrix.</param>
        /// <param name="offsetY">Value of the Y offset field of the new matrix.</param>
        /// <param name="offsetZ">Value of the Z offset field of the new matrix.</param>
        /// <param name="m44">Value of the (4,4) field of the new matrix.</param>
        public Matrix3D(double m11, double m12, double m13, double m14, double m21, double m22, double m23, double m24, double m31, double m32, double m33, double m34, double offsetX, double offsetY, double offsetZ, double m44)
        {
           this.mM11 = m11;
           this.mM12 = m12;
           this.mM13 = m13;
           this.mM14 = m14;

           this.mM21 = m21;
           this.mM22 = m22;
           this.mM23 = m23;
           this.mM24 = m24;

           this.mM31 = m31;
           this.mM32 = m32;
           this.mM33 = m33;
           this.mM34 = m34;

           this.mOffsetX = offsetX;
           this.mOffsetY = offsetY;
           this.mOffsetZ = offsetZ;
           this.mM44 = m44;
           this._IsDefaultMatrix = false;
        }
        
        #endregion

        #region IFormattable Members

        public string ToString(string format, IFormatProvider formatProvider)
        {
            return this.ToString(formatProvider);
        }

        #endregion

        #region Operators
        
        /// <summary>
        /// Compares two Matrix3D instances for exact equality.
        /// </summary>
        /// <param name="matrix1">The first matrix to compare.</param>
        /// <param name="matrix2">The second matrix to compare.</param>
        /// <returns></returns>
        public static bool operator ==(Matrix3D matrix1, Matrix3D matrix2)
        {
            return matrix1.Equals(matrix2);
        }

        /// <summary>
        /// Compares two Matrix3D instances for exact inequality.
        /// </summary>
        /// <param name="matrix1">The first matrix to compare.</param>
        /// <param name="matrix2">The second matrix to compare.</param>
        /// <returns>Boolean that indicates whether the two matrix instances are inequal.</returns>
        public static bool operator !=(Matrix3D matrix1, Matrix3D matrix2)
        {
            return !Matrix3D.Equals(matrix1, matrix2);
        }

        /// <summary>
        /// Multiplies the specified matrices.
        /// </summary>
        /// <param name="matrix1">Matrix to multiply.</param>
        /// <param name="matrix2">Matrix by which the first matrix is multiplied.</param>
        /// <returns>Matrix3D that is the result of multiplication.</returns>
        public static Matrix3D operator *(Matrix3D matrix1, Matrix3D matrix2)
        {
            return Matrix3D.Multiply(matrix1, matrix2);
        }

        #endregion

        #region Methods
        /// <summary>
        /// Appends a specified matrix to the current matrix.
        /// </summary>
        /// <param name="matrix">Matrix to append.</param>
        /// <remarks>
        /// Matrices can be appended or prepended to other matrices. Appending matrix A to matrix B denotes a transformation by B and then by A.
        /// To transform point P by A and then by B, append B to A.
        /// </remarks>
        public void Append(Matrix3D matrix)
        {
            this = Matrix3D.Multiply(this, matrix);
        }

        /// <summary>
        /// Prepends a specified matrix to the current matrix.
        /// </summary>
        /// <param name="matrix">Matrix to prepend.</param>
        public void Prepend(Matrix3D matrix)
        {
            this = Matrix3D.Multiply(matrix, this);
        }
        
        /// <summary>
        /// Tests equality between two matrices.
        /// </summary>
        /// <param name="o">Object to test for equality.</param>
        /// <returns>Boolean that indicates whether the matrices are equal.</returns>
        public override bool Equals(object o)
        {
            if (o is Matrix3D)
            {
                return this.Equals((Matrix3D)o);
            }
            else
            {
                return false;
            }
        }

        /// <summary>
        /// Tests equality between two matrices.
        /// </summary>
        /// <param name="value">The Matrix3D to which to compare.</param>
        /// <returns>Boolean that indicates whether the matrices are equal.</returns>
        public bool Equals(Matrix3D value)
        {
            return this.mM11 == value.mM11 && this.mM12 == value.mM12 && this.mM13 == value.mM13 && this.mM14 == value.mM14 &&
                   this.mM21 == value.mM21 && this.mM22 == value.mM22 && this.mM23 == value.mM23 && this.mM24 == value.mM24 &&
                   this.mM31 == value.mM31 && this.mM32 == value.mM32 && this.mM33 == value.mM33 && this.mM34 == value.mM34 &&
                   this.mOffsetX == value.mOffsetX && this.mOffsetY == value.mOffsetY && this.mOffsetZ == value.mOffsetZ && this.mM44 == value.mM44;
        }

        /// <summary>
        /// Tests equality between two matrices.
        /// </summary>
        /// <param name="matrix1">The first Matrix3D to compare.</param>
        /// <param name="matrix2">The second Matrix3D to compare.</param>
        /// <returns>Boolean that indicates whether the matrices are equal.</returns>
        public static bool Equals(Matrix3D matrix1, Matrix3D matrix2)
        {
            return matrix1.Equals(matrix2);
        }

        /// <summary>
        /// Inverts this Matrix3D structure.
        /// </summary>
        /// <remarks>To determine whether a Matrix3D structure is invertible, read its HasInverse property.</remarks>
        public void Invert()
        {
            // TODO: Obtimize Is default matrix.
            if (this.IsIdentity)
            {
                // The matrix is the identity matrix, nothing to do
                return;
            }
            else
            {
               
                double determinant = this.Determinant;
                if (System.Math.Round(determinant, 4) == 0)
                {
                    throw new InvalidOperationException("The matrix does not have an inverse");
                }

                double num1 = mM33 * mM44 - mM34 * mOffsetZ;
                double num2 = mM32 * mM44 - mM34 * mOffsetY;
                double num3 = mM31 * mM44 - mM34 * mOffsetX;
                double num4 = mM32 * mOffsetZ - mM33 * mOffsetY;
                double num5 = mM31 * mOffsetZ - mM33 * mOffsetX;
                double num6 = mM31 * mOffsetY - mM32 * mOffsetX;

                double num7 = mM33 * mM44 - mM34 * mOffsetZ;
                double num8 = mM32 * mM44 - mM34 * mOffsetY;
                double num9 = mM31 * mM44 - mM34 * mOffsetX;
                double num10 = mM32 * mOffsetZ - mM33 * mOffsetY;
                double num11 = mM31 * mOffsetZ - mM33 * mOffsetX;
                double num12 = mM31 * mOffsetY - mM32 * mOffsetX;

                double num13 = mM23 * mM44 - mM24 * mOffsetZ;
                double num14 = mM22 * mM44 - mM24 * mOffsetY;
                double num15 = mM21 * mM44 - mM24 * mOffsetX;
                double num16 = mM22 * mOffsetZ - mM23 * mOffsetY;
                double num17 = mM21 * mOffsetZ - mM23 * mOffsetX;
                double num18 = mM21 * mOffsetY - mM22 * mOffsetX;

                double num19 = mM23 * mM34 - mM24 * mM33;
                double num20 = mM22 * mM34 - mM24 * mM32;
                double num21 = mM21 * mM34 - mM24 * mM31;
                double num22 = mM22 * mM33 - mM23 * mM32;
                double num23 = mM21 * mM33 - mM23 * mM31;
                double num24 = mM21 * mM32 - mM22 * mM31;

                double cofactor11 = (mM22 * num1 - mM23 * num2 + mM24 * num4);
                double cofactor12 = -(mM21 * num1 - mM23 * num3 + mM24 * num5);
                double cofactor13 = (mM21 * num2 - mM22 * num3 + mM24 * num6);
                double cofactor14 = -(mM21 * num4 - mM22 * num5 + mM23 * num6);

                double cofactor21 = -(mM12 * num7 - mM13 * num8 + mM14 * num10);
                double cofactor22 = (mM11 * num7 - mM13 * num9 + mM14 * num11);
                double cofactor23 = -(mM11 * num8 - mM12 * num9 + mM14 * num12);
                double cofactor24 = (mM11 * num10 - mM12 * num11 + mM13 * num12);

                double cofactor31 = (mM12 * num13 - mM13 * num14 + mM14 * num16);
                double cofactor32 = -(mM11 * num13 - mM13 * num15 + mM14 * num17);
                double cofactor33 = (mM11 * num14 - mM12 * num15 + mM14 * num18);
                double cofactor34 = -(mM11 * num16 - mM12 * num17 + mM13 * num18);

                double cofactor41 = -(mM12 * num19 - mM13 * num20 + mM14 * num22);
                double cofactor42 = (mM11 * num19 - mM13 * num21 + mM14 * num23);
                double cofactor43 = -(mM11 * num20 - mM12 * num21 + mM14 * num24);
                double cofactor44 = (mM11 * num22 - mM12 * num23 + mM13 * num24);

                double inverseDet = 1.0 / determinant;
                this.mM11 = cofactor11 * inverseDet;
                this.mM12 = cofactor21 * inverseDet;
                this.mM13 = cofactor31 * inverseDet;
                this.mM14 = cofactor41 * inverseDet;
                this.mM21 = cofactor12 * inverseDet;
                this.mM22 = cofactor22 * inverseDet;
                this.mM23 = cofactor32 * inverseDet;
                this.mM24 = cofactor42 * inverseDet;
                this.mM31 = cofactor13 * inverseDet;
                this.mM32 = cofactor23 * inverseDet;
                this.mM33 = cofactor33 * inverseDet;
                this.mM34 = cofactor43 * inverseDet;
                this.mOffsetX = cofactor14 * inverseDet;
                this.mOffsetY = cofactor24 * inverseDet;
                this.mOffsetZ = cofactor34 * inverseDet;
                this.mM44 = cofactor44 * inverseDet;
            }
        }

        /// <summary>
        /// Multiplies the specified matrices.
        /// </summary>
        /// <param name="matrix1">Matrix to multiply.</param>
        /// <param name="matrix2">Matrix by which the first matrix is multiplied.</param>
        /// <returns>Matrix3D that is the result of multiplication.</returns>
        public static Matrix3D Multiply(Matrix3D matrix1, Matrix3D matrix2)
        {
            if (matrix1.IsIdentity)
            {
                return matrix2;
            }
            else if (matrix2.IsIdentity)
            {
                return matrix1; 
            }
            else if (matrix1.IsAffine)
            {
                return new Matrix3D(matrix1.mM11 * matrix2.mM11 + matrix1.mM12 * matrix2.mM21 + matrix1.mM13 * matrix2.mM31,
                                    matrix1.mM11 * matrix2.mM12 + matrix1.mM12 * matrix2.mM22 + matrix1.mM13 * matrix2.mM32,
                                    matrix1.mM11 * matrix2.mM13 + matrix1.mM12 * matrix2.mM23 + matrix1.mM13 * matrix2.mM33,
                                    matrix1.mM11 * matrix2.mM14 + matrix1.mM12 * matrix2.mM24 + matrix1.mM13 * matrix2.mM34,
                                    matrix1.mM21 * matrix2.mM11 + matrix1.mM22 * matrix2.mM21 + matrix1.mM23 * matrix2.mM31,
                                    matrix1.mM21 * matrix2.mM12 + matrix1.mM22 * matrix2.mM22 + matrix1.mM23 * matrix2.mM32,
                                    matrix1.mM21 * matrix2.mM13 + matrix1.mM22 * matrix2.mM23 + matrix1.mM23 * matrix2.mM33,
                                    matrix1.mM21 * matrix2.mM14 + matrix1.mM22 * matrix2.mM24 + matrix1.mM23 * matrix2.mM34,
                                    matrix1.mM31 * matrix2.mM11 + matrix1.mM32 * matrix2.mM21 + matrix1.mM33 * matrix2.mM31,
                                    matrix1.mM31 * matrix2.mM12 + matrix1.mM32 * matrix2.mM22 + matrix1.mM33 * matrix2.mM32,
                                    matrix1.mM31 * matrix2.mM13 + matrix1.mM32 * matrix2.mM23 + matrix1.mM33 * matrix2.mM33,
                                    matrix1.mM31 * matrix2.mM14 + matrix1.mM32 * matrix2.mM24 + matrix1.mM33 * matrix2.mM34,
                                    matrix1.mOffsetX * matrix2.mM11 + matrix1.mOffsetY * matrix2.mM21 + matrix1.mOffsetZ * matrix2.mM31 + matrix1.mM44 * matrix2.mOffsetX,
                                    matrix1.mOffsetX * matrix2.mM12 + matrix1.mOffsetY * matrix2.mM22 + matrix1.mOffsetZ * matrix2.mM32 + matrix1.mM44 * matrix2.mOffsetY,
                                    matrix1.mOffsetX * matrix2.mM13 + matrix1.mOffsetY * matrix2.mM23 + matrix1.mOffsetZ * matrix2.mM33 + matrix1.mM44 * matrix2.mOffsetZ,
                                    matrix1.mOffsetX * matrix2.mM14 + matrix1.mOffsetY * matrix2.mM24 + matrix1.mOffsetZ * matrix2.mM34 + matrix1.mM44 * matrix2.mM44);
            }
            else 
            {
                return new Matrix3D(matrix1.mM11 * matrix2.mM11 + matrix1.mM12 * matrix2.mM21 + matrix1.mM13 * matrix2.mM31 + matrix1.mM14 * matrix2.mOffsetX,
                                matrix1.mM11 * matrix2.mM12 + matrix1.mM12 * matrix2.mM22 + matrix1.mM13 * matrix2.mM32 + matrix1.mM14 * matrix2.mOffsetY,
                                matrix1.mM11 * matrix2.mM13 + matrix1.mM12 * matrix2.mM23 + matrix1.mM13 * matrix2.mM33 + matrix1.mM14 * matrix2.mOffsetZ,
                                matrix1.mM11 * matrix2.mM14 + matrix1.mM12 * matrix2.mM24 + matrix1.mM13 * matrix2.mM34 + matrix1.mM14 * matrix2.mM44,
                                matrix1.mM21 * matrix2.mM11 + matrix1.mM22 * matrix2.mM21 + matrix1.mM23 * matrix2.mM31 + matrix1.mM24 * matrix2.mOffsetX,
                                matrix1.mM21 * matrix2.mM12 + matrix1.mM22 * matrix2.mM22 + matrix1.mM23 * matrix2.mM32 + matrix1.mM24 * matrix2.mOffsetY,
                                matrix1.mM21 * matrix2.mM13 + matrix1.mM22 * matrix2.mM23 + matrix1.mM23 * matrix2.mM33 + matrix1.mM24 * matrix2.mOffsetZ,
                                matrix1.mM21 * matrix2.mM14 + matrix1.mM22 * matrix2.mM24 + matrix1.mM23 * matrix2.mM34 + matrix1.mM24 * matrix2.mM44,
                                matrix1.mM31 * matrix2.mM11 + matrix1.mM32 * matrix2.mM21 + matrix1.mM33 * matrix2.mM31 + matrix1.mM34 * matrix2.mOffsetX,
                                matrix1.mM31 * matrix2.mM12 + matrix1.mM32 * matrix2.mM22 + matrix1.mM33 * matrix2.mM32 + matrix1.mM34 * matrix2.mOffsetY,
                                matrix1.mM31 * matrix2.mM13 + matrix1.mM32 * matrix2.mM23 + matrix1.mM33 * matrix2.mM33 + matrix1.mM34 * matrix2.mOffsetZ,
                                matrix1.mM31 * matrix2.mM14 + matrix1.mM32 * matrix2.mM24 + matrix1.mM33 * matrix2.mM34 + matrix1.mM34 * matrix2.mM44,
                                matrix1.mOffsetX * matrix2.mM11 + matrix1.mOffsetY * matrix2.mM21 + matrix1.mOffsetZ * matrix2.mM31 + matrix1.mM44 * matrix2.mOffsetX,
                                matrix1.mOffsetX * matrix2.mM12 + matrix1.mOffsetY * matrix2.mM22 + matrix1.mOffsetZ * matrix2.mM32 + matrix1.mM44 * matrix2.mOffsetY,
                                matrix1.mOffsetX * matrix2.mM13 + matrix1.mOffsetY * matrix2.mM23 + matrix1.mOffsetZ * matrix2.mM33 + matrix1.mM44 * matrix2.mOffsetZ,
                                matrix1.mOffsetX * matrix2.mM14 + matrix1.mOffsetY * matrix2.mM24 + matrix1.mOffsetZ * matrix2.mM34 + matrix1.mM44 * matrix2.mM44);
            }
        }
        
        /// <summary>
        /// Converts a string representation of a Matrix3D structure into the equivalent Matrix3D structure.
        /// </summary>
        /// <param name="source">String representation of the Matrix3D.</param>
        /// <returns>Matrix3D structure represented by the string.</returns>
        public static Matrix3D Parse(string source)
        {
            if (source.Trim() != "")
            {
                string[] vals = source.Split(',');
                if (vals.Length == 16)
                {
                    // The string is expressed by columns: M11, M12, M13, M14, M21, M22, M23, M24, M31, M32, M33, M34, OffsetX, OffsetY, OffsetZ, M44
                    Matrix3D result = new Matrix3D(double.Parse(vals[0]), double.Parse(vals[1]), double.Parse(vals[2]), double.Parse(vals[3]),
                                                   double.Parse(vals[4]), double.Parse(vals[5]), double.Parse(vals[6]), double.Parse(vals[7]),
                                                   double.Parse(vals[8]), double.Parse(vals[9]), double.Parse(vals[10]), double.Parse(vals[11]),
                                                   double.Parse(vals[12]), double.Parse(vals[13]), double.Parse(vals[14]), double.Parse(vals[15]));
                    return result;
                }
                else
                {
                    throw new FormatException();
                }
            }
            else
            {
                return Matrix3D.Identity;
            }
        }

        /// <summary>
        /// Appends the specified scale Vector3D to this Matrix3D structure.
        /// </summary>
        /// <param name="scale">Vector3D by which to scale this Matrix3D structure.</param>
        public void Scale(Vector3D scale)
        {
            if (this.IsIdentity)
            {
                this.M11 = scale.X;
                this.M22 = scale.Y;
                this.M33 = scale.Z;
                this.M44 = 1;
            }
            else
            {
                this.mM11 *= scale.X;
                this.mM12 *= scale.Y;
                this.mM13 *= scale.Z;
                this.mM21 *= scale.X;
                this.mM22 *= scale.Y;
                this.mM23 *= scale.Z;
                this.mM31 *= scale.X;
                this.mM32 *= scale.Y;
                this.mM33 *= scale.Z;
                this.mOffsetX *= scale.X;
                this.mOffsetY *= scale.Y;
                this.mOffsetZ *= scale.Z;
            }
        }

        /// <summary>
        /// Scales this Matrix3D structure by the specified Vector3D about the specified Point3D.
        /// </summary>
        /// <param name="scale">Vector3D by which to scale this Matrix3D structure.</param>
        /// <param name="center">Point3D about which to scale.</param>
        public void ScaleAt(Vector3D scale, Point3D center)
        {
            if (this.IsIdentity)
            {
                this.mM11 = scale.X;
                this.mM22 = scale.Y;
                this.mM33 = scale.Z;
                this.mM44 = 1.0;
                this.mOffsetX = center.X - (center.X * scale.X);
                this.mOffsetY = center.Y - (center.Y * scale.Y);
                this.mOffsetZ = center.Z - (center.Z * scale.Z);
            }
            else
            {
                this.mM11 = this.mM11 * scale.X + this.mM14 * center.X - this.mM14 * scale.X * center.X;
                this.mM12 = this.mM12 * scale.Y + this.mM14 * center.Y - this.mM14 * scale.Y * center.Y;
                this.mM13 = this.mM13 * scale.Z + this.mM14 * center.Z - this.mM14 * scale.Z * center.Z;

                this.mM21 = this.mM21 * scale.X + this.mM24 * center.X - this.mM24 * scale.X * center.X;
                this.mM22 = this.mM22 * scale.Y + this.mM24 * center.Y - this.mM24 * scale.Y * center.Y;
                this.mM23 = this.mM23 * scale.Z + this.mM24 * center.Z - this.mM24 * scale.Z * center.Z;

                this.mM31 = this.mM31 * scale.X + this.mM34 * center.X - this.mM34 * scale.X * center.X;
                this.mM32 = this.mM32 * scale.Y + this.mM34 * center.Y - this.mM34 * scale.Y * center.Y;
                this.mM33 = this.mM33 * scale.Z + this.mM34 * center.Z - this.mM34 * scale.Z * center.Z;

                this.mOffsetX = this.mOffsetX * scale.X + this.mM44 * center.X - this.mM44 * scale.X * center.X;
                this.mOffsetY = this.mOffsetY * scale.Y + this.mM44 * center.Y - this.mM44 * scale.Y * center.Y;
                this.mOffsetZ = this.mOffsetZ * scale.Z + this.mM44 * center.Z - this.mM44 * scale.Z * center.Z;
            }
        }

        /// <summary>
        /// Appends a translation of the specified offset to the current Matrix3D structure.
        /// </summary>
        /// <param name="offset">Vector3D that specifies the offset for transformation.</param>
        public void Translate(Vector3D offset)
        {
            // TODO: Review this funtion. This is true for Identity matrix, but for non identity problably i need to create a identity matrix with this values as offset and then multiply it with this.
            this.mOffsetX += offset.X;
            this.mOffsetY += offset.Y;
            this.mOffsetZ += offset.Z;
        }

        /// <summary>
        /// Creates a string representation of this Matrix3D structure.
        /// </summary>
        /// <returns>String representation of this Matrix3D structure.</returns>
        public override string ToString()
        {
            return string.Format("{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15}",
                                 this.mM11, this.mM12, this.mM13, this.mM14, this.mM21, this.mM22, this.mM23,
                                 this.mM24, this.mM31, this.mM32, this.mM33, this.mM34,
                                 this.mOffsetX, this.mOffsetY, this.mOffsetZ, this.mM44);
        }

        /// <summary>
        /// Creates a string representation of this Matrix3D structure.
        /// </summary>
        /// <param name="provider">Culture-specified formatting information.</param>
        /// <returns>String representation of this Matrix3D structure.</returns>
        public string ToString(IFormatProvider provider)
        {
            return string.Format(provider, "{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},{14},{15}",
                                 this.mM11, this.mM12, this.mM13, this.mM14, this.mM21, this.mM22, this.mM23,
                                 this.mM24, this.mM31, this.mM32, this.mM33, this.mM34,
                                 this.mOffsetX, this.mOffsetY, this.mOffsetZ, this.mM44);
        }

        /// <summary>
        /// Transforms the specified Point3D by the Matrix3D and returns the result
        /// </summary>
        /// <param name="point">Point3D to transform.</param>
        /// <returns>The result of transforming point by this Matrix3D</returns>
        public Point3D Transform(Point3D point)
        {
            if (this.IsIdentity)
            {
                return point;
            }
            else
            {
                Point3D pt = new Point3D(this.mM11 * point.X + this.mM21 * point.Y + this.mM31 * point.Z + this.mOffsetX,
                                   this.mM12 * point.X + this.mM22 * point.Y + this.mM32 * point.Z + this.mOffsetY,
                                   this.mM13 * point.X + this.mM23 * point.Y + this.mM33 * point.Z + this.mOffsetZ);

                if (!this.IsAffine)
                {
                    double d = 1.0 / (point.X * this.mM14 + point.Y * this.mM24 + point.Z * this.mM34 + this.mM44);
                    pt.X *= d;
                    pt.Y *= d;
                    pt.Z *= d;
                }

                return pt;
            }
        }

        /// <summary>
        /// Transforms the specified Vector3D by this Matrix3D.
        /// </summary>
        /// <param name="vector">Vector3D to transform.</param>
        /// <returns>The result of transforming vector by this Matrix3D.</returns>
        public Vector3D Transform(Vector3D vector)
        {
            if (this.IsIdentity)
            {
                return vector;
            }

            return new Vector3D(vector.X * this.mM11 + vector.Y * this.mM21 + vector.Z * this.mM31,
                                vector.X * this.mM12 + vector.Y * this.mM22 + vector.Z * this.mM32,
                                vector.X * this.mM13 + vector.Y * this.mM23 + vector.Z * this.mM33);
        }
        
        /// <summary>
        /// Transforms the specified Point3D objects in the array by the Matrix3D.
        /// </summary>
        /// <param name="points">Point3D objects to transform. The original points in the array are replaced by their transformed values.</param>
        public void Transform(Point3D[] points)
        {
            for (int i = 0; i < points.Length; i++)
            {
                points[i] = this.Transform(points[i]);
            }
        }

        /// <summary>
        /// Transforms the specified Vector3D objects in the array by this Matrix3D.
        /// </summary>
        /// <param name="vectors">Vector3D objects to transform. The original Vector3D objects in the array are replaced by their transformed values.</param>
        public void Transform(Vector3D[] vectors)
        {
            for (int i = 0; i < vectors.Length; i++)
            {
                vectors[i] = this.Transform(vectors[i]);
            }
        }

        /// <summary>
        /// For internal use only, expects that X,Y,Z is the min point extreme
        /// of the rectangle.
        /// </summary>
        /// <param name="rect"></param>
        /// <returns></returns>
        internal Rect3D Transform(Rect3D rect)
        {
            // Need to transform all the points and find the new extremes
            Point3D p0 = new Point3D(rect.X, rect.Y, rect.Z);
            Point3D p1 = new Point3D(rect.X + rect.SizeX, rect.Y, rect.Z);
            Point3D p2 = new Point3D(rect.X + rect.SizeX, rect.Y + rect.SizeY, rect.Z);
            Point3D p3 = new Point3D(rect.X, rect.Y + rect.SizeY, rect.Z);

            Point3D p4 = new Point3D(rect.X, rect.Y, rect.Z + rect.SizeZ);
            Point3D p5 = new Point3D(rect.X + rect.SizeX, rect.Y, rect.Z + rect.SizeZ);
            Point3D p6 = new Point3D(rect.X + rect.SizeX, rect.Y + rect.SizeY, rect.Z + rect.SizeZ);
            Point3D p7 = new Point3D(rect.X, rect.Y + rect.SizeY, rect.Z + rect.SizeZ);

            Rect3D rectNew = Rect3D.Empty;
            rectNew.Union(this.Transform(p0));
            rectNew.Union(this.Transform(p1));
            rectNew.Union(this.Transform(p2));
            rectNew.Union(this.Transform(p3));
            rectNew.Union(this.Transform(p4));
            rectNew.Union(this.Transform(p5));
            rectNew.Union(this.Transform(p6));
            rectNew.Union(this.Transform(p7));

            return rectNew;
        }
        
        /// <summary>
        /// Returns the hash code for this matrix
        /// </summary>
        /// <returns>Integer that specifies the hash code for this matrix.</returns>
        public override int GetHashCode()
        {
            return this.mM11.GetHashCode() | this.mM12.GetHashCode() | this.mM13.GetHashCode() | this.mM14.GetHashCode() |
                   this.mM21.GetHashCode() | this.mM22.GetHashCode() | this.mM23.GetHashCode() | this.mM24.GetHashCode() |
                   this.mM31.GetHashCode() | this.mM32.GetHashCode() | this.mM33.GetHashCode() | this.mM34.GetHashCode() |
                   this.mOffsetX.GetHashCode() | this.mOffsetY.GetHashCode() | this.mOffsetZ.GetHashCode() | this.mM44.GetHashCode();
        }
        #endregion
    }
}
